﻿using Accord.Math;
using AMEE.MashineLearning.Geometry; // RansacPlane
using AMEE.PlaneDetection.Kinect; // Mein PointCloud K4W Wrapper
using System;
using System.Threading.Tasks;
using System.Diagnostics;
using System.Text;
using System.Threading;


namespace AMEE.PlaneDetection
{
    class Program
    {
        static RansacPlane ransac;
        static Stopwatch _stopWatch;

        /// <summary>
        /// Testprogramm: Gib Punktwolke und finde Fläche.
        /// <para>
        /// Start und Ende des Programms durch Tastendruck.
        /// </para>
        /// <para>
        /// Nimmt den <c>K4Wv2PointCloud</c> Reader. Beim event <c>PointCloudReady</c> wird mit einem
        /// <c>RansacPlane</c> eine möglichst 'gute' Fläche detektiert und deren Normalenvektor auf der Konsole ausgegeben.
        /// <c>RansacPlane</c> stammt aus Accord.net und wurde von Björn ein wenig gepimpt ohne dabei den Algorithmus
        /// zu ändern.
        /// </para>
        /// </summary>
        /// <param name="args">The arguments.</param>
        static void Main(string[] args) {

            //K4Wv2PointCloudTest();

            // RansacPlaneTest();

            ScanOnceTest();
        }

        private static void ScanOnceTest() {
            PlaneFinder finder = new PlaneFinder();
            finder.RansacThreshold = 0.005; // 5 mm
            finder.RansacProbability = 0.90;
            finder.FrameRate = 0.5; // 1 Frame je 2 Sekunden

            finder.PlaneDetected += (sender, planeEvArgs) => {
                StringBuilder sb = new StringBuilder();
                sb.AppendLine(string.Format("Found Plane: {0:f}*x + {1:f}*y + {2:f}*z + {3:f}",
                    planeEvArgs.Plane.A, planeEvArgs.Plane.B, planeEvArgs.Plane.C, planeEvArgs.Plane.Offset));
                sb.AppendFormat("Num of inliers: {0} von {1}",
                    planeEvArgs.Inliers.Length, planeEvArgs.PointCloud.Length);
                sb.AppendLine();
                Console.WriteLine(sb.ToString());
            };

            finder.NoPlaneFound += (sender, e) => {
                Console.WriteLine("No plane found");
            };

            ConsoleKeyInfo keyInfo;
            do {
                Console.WriteLine("K4W v2 starten und Punktwolken erzeugen.\nTaste drücken, um fortzufahren...\nEscape zum beenden.");
                //Console.WriteLine("Nach Start beliebige Taste erneut drücken, um zu pausieren.\n");
                keyInfo = Console.ReadKey(false);
                if (keyInfo.Key == ConsoleKey.Escape) {
                    finder.Stop();
                    break; // beende Schleife und Programm 
                }
                bool continu = false;
                do {

                    finder.ScanOnce();
                    Thread.Sleep((int)(1000.0 / finder.FrameRate) + 500);
                    //var task = finder.ScanOnceAsync();
                    ////task.Start();
                    //task.Wait(4000);
                    //if (task.IsCanceled || task.IsFaulted) {
                    //    Console.WriteLine("Fehler");
                    //    continue;
                    //}
                    //var planeEvArgs = task.Result;
                    //if (planeEvArgs == null) {
                    //    Console.WriteLine("no plane");
                    //    continue;
                    //}
                    //StringBuilder sb = new StringBuilder();
                    //sb.AppendLine(string.Format("Found Plane: {0:f}*x + {1:f}*y + {2:f}*z + {3:f}",
                    //    planeEvArgs.Plane.A, planeEvArgs.Plane.B, planeEvArgs.Plane.C, planeEvArgs.Plane.Offset));
                    //sb.AppendFormat("Num of inliers: {0} von {1}",
                    //    planeEvArgs.Inliers.Length, planeEvArgs.PointCloud.Length);
                    //sb.AppendLine();
                    //Console.WriteLine(sb.ToString());

                    bool korrekteEingabe = false;
                    do {

                        Console.Write("K4W v2 pausiert. [c]ontinue. [s]top. : ");
                        keyInfo = Console.ReadKey();
                        switch (keyInfo.Key) {
                            case ConsoleKey.S:
                                // 
                                break;
                            case ConsoleKey.C:
                                //
                                break;
                            default:
                                break;
                        }
                        Console.WriteLine();
                        korrekteEingabe = keyInfo.Key == ConsoleKey.S || keyInfo.Key == ConsoleKey.C;
                        continu = keyInfo.Key == ConsoleKey.C;
                    } while (!korrekteEingabe);
                } while (continu);
                finder.Stop();
                Console.WriteLine("\n------------------\n");
            } while (true);

        }

        private static void RansacPlaneTest() {
            double threshold = 0.005; // 5mm (bzw. 0,005 m)
            double probability = 0.97; // 97 % (üblich sind 0,95 - 0,99)
            ransac = new RansacPlane(threshold, probability);
            var ptCloudReader = new K4Wv2PointCloud();
            // FrameRate
            //ptCloudReader.FrameRate = 1;
            // SkippedFrames
            ptCloudReader.SkippedFrames = 89; // ca. alle 3 Sek. 
            ptCloudReader.PointCloudReady += ptCloudReader_PointCloudReady;

            Console.WriteLine("K4W v2 starten und Punktwolken erzeugen.\nTaste drücken, um fortzufahren...\n");
            Console.ReadKey(false);

            bool isInit = ptCloudReader.Initialize();


            Console.WriteLine("K4Wv2PointCloud Initialisiert? " + (isInit ? "ja" : "NEIN!"));
            Console.ReadKey(false);

            ptCloudReader.Stop();


            Console.WriteLine("Kinect und PCld-Leser gestoppt.\nTaste drücken um zu beenden...");
            Console.ReadKey();
        }

        static void ptCloudReader_PointCloudReady(object sender, PointCloudReadyEventArg e) {
            var k4wPoints = e.PointCloud;
            Point3[] theCloud = k4wPoints.ToAccord();

            //int num = 0;
            //foreach (var point in theCloud) {
            //    if (!float.IsInfinity(point.Z)) {
            //        num++;
            //    }
            //}
            //Console.WriteLine(num + " / " + theCloud.Length);

            var task = new Task(pcl => {
                Point3[] points = (Point3[])pcl;
                var pRansac = new RansacPlane(0.005, 0.97);
                Accord.Math.Plane plane = pRansac.Estimate(points);
                if (plane != null) {
                    Console.WriteLine("Ebenennormale = ({0}, {1}, {2})\n\tOffset = {3}", plane.Normal.X, plane.Normal.Y, plane.Normal.Z, plane.Offset);
                }
                else {
                    Console.WriteLine("no plane.");
                }
            }, theCloud);
            task.Start();

            //Thread t = new Thread(pcl => {
            //    Point3[] points = (Point3[])pcl;
            //    var pRansac = new nspBeRu.Geometry.RansacPlane(0.005, 0.97);
            //    Plane plane = pRansac.Estimate(points);
            //    if (plane != null) {
            //        Console.WriteLine("Ebenennormale = ({0}, {1}, {2})", plane.Normal.X, plane.Normal.Y, plane.Normal.Z);
            //        Console.WriteLine("\tOffset = " + plane.Offset);
            //    }
            //    else {
            //        Console.WriteLine("no plane.");
            //    }
            //});
            //t.Start(theCloud);

            //Plane plane = ransac.Estimate(theCloud);
            //Console.WriteLine("Ebenennormale = ({0}, {1}, {2})", plane.Normal.X, plane.Normal.Y, plane.Normal.Z);
            //Console.WriteLine("\tOffset = " + plane.Offset);
        }

        static void K4Wv2PointCloudTest() {
            var pclReader = new K4Wv2PointCloud();
            pclReader.PointCloudReady += pclReader_PointCloudReady;

            _stopWatch = new Stopwatch();
            // FrameRate
            pclReader.FrameRate = 5;
            // SkippedFrames
            pclReader.SkippedFrames = 20;

            ConsoleKeyInfo keyInfo;
            do {
                pclReader.Stop();
                Console.WriteLine("K4W v2 starten und Punktwolken erzeugen.\nTaste drücken, um fortzufahren...\nEscape zum beenden.");
                Console.WriteLine("Nach Start beliebige Taste erneut drücken, um zu pausieren.\n");
                keyInfo = Console.ReadKey(false);
                if (keyInfo.Key == ConsoleKey.Escape) {
                    break; // beende Schleife und Programm 
                }
                _stopWatch.Start();
                pclReader.Initialize();

                
                // Traum, runterbrechen auf Versuchsaufbau, Grundlagen (sofern für das hiesige Verständnis nötig)
                // Analyse - Vergleich mit anderen Arbeiten (mit kritischer rezeption)
                bool continu = false;
                do {

                    Console.ReadKey(false);
                    pclReader.Pause();
                    _stopWatch.Stop();
                    bool korrekteEingabe = false;
                    do {

                        Console.Write("K4W v2 pausiert. [c]ontinue. [s]top. : ");
                        keyInfo = Console.ReadKey();
                        switch (keyInfo.Key) {
                            case ConsoleKey.S:
                                pclReader.Stop();
                                // 
                                break;
                            case ConsoleKey.C:
                                pclReader.Continue();
                                _stopWatch.Restart();
                                break;
                            default:
                                break;
                        }
                        Console.WriteLine();
                        korrekteEingabe = keyInfo.Key == ConsoleKey.S || keyInfo.Key == ConsoleKey.C;
                        continu = keyInfo.Key == ConsoleKey.C;
                    } while (!korrekteEingabe);
                } while (continu);
                Console.WriteLine("\n------------------\n");
            } while (true);
        }

        static void pclReader_PointCloudReady(object sender, PointCloudReadyEventArg e) {
            _stopWatch.Stop();
            long millis = _stopWatch.ElapsedMilliseconds;
            _stopWatch.Restart();
            double frameRate = 1000.0 / millis;
            var cloud = e.PointCloud;
            int zets = 0;
            foreach (var point in cloud) {
                if (float.IsInfinity(point.Z)) {
                    zets++;
                }
            }
            Console.WriteLine("Frame Rate: {0:f}\nUnendliche Punkte: {1} von {2}", frameRate, zets, cloud.Length);
        }
    }
}
